#include <ros/ros.h>
#include <thread>
#include <cstring>
#include <iostream>
#include <serial/serial.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <csignal>
#include <Eigen/Dense>

#include "printf_utils.h"
#include "math_utils.h"

//demo 程序
//功能： 1) 发送状态控制指令到Mini控制启停算法等
//      2) 解码Mini发送的姿态数据,打印出来或者打包成ROS话题发送出去

#define LREV_LEN    61
#define LSEND_LEN   34
#define LS_CHECK    LSEND_LEN - 3   //发送校验位序列
#define LR_CHECK    LREV_LEN - 3    //接收校验位置

#define RHEAD_1     0X66    //接收帧头
#define RHEAD_2     0X27
#define RTILE_1     0X08
#define RTILE_2     0X0a   

ros::Publisher pub_viobot_odom;
nav_msgs::Odometry viobot_odom;

serial::Serial serial_port;
string port_name;
uint8_t send_buffer[LSEND_LEN];
uint8_t receive_buffer[LREV_LEN];
ros::Time time_last;

struct pose_t
{
    float px,py,pz,qx,qy,qz,qw;
};

struct speed_t
{
    float lx,ly,lz,ax,ay,az;
};

struct odom_t
{
    pose_t pose;
    speed_t speed;
};

enum algo_state
{
    algo_enable = 0x01,     //启动
    algo_disable = 0x02,    //停止
    algo_restart = 0x03,    //重置
    algo_none   = 0x00,
};

void signal_handle(int sig);
void serial_init();
void send_command(const algo_state t);
void serial_receive_thread();
bool unpack_uart(odom_t& odom,int& frame_id);
void publish_odom(const odom_t& odom);
Eigen::Vector3d Q_to_euler(Eigen::Quaterniond Q);

int main(int argc,char **argv)
{
    ros::init(argc,argv,"mini_serial");
    ros::NodeHandle nh;
    ros::Rate rate(100);
    signal(SIGINT,signal_handle);

    nh.param<std::string>("port_name", port_name, "/dev/ttyUSB0");

    // 串口初始化操作
	serial_init();

    // 发布odom话题
    pub_viobot_odom = nh.advertise<nav_msgs::Odometry>("/sunray/robobaton_mini/odom", 10);

    // 接收串口线程
    std::thread receive_thread{serial_receive_thread};

    // 直接启动算法
    send_command(algo_enable);
    cout << GREEN << "Robobaton mini odom enable..." << TAIL << endl;

    while(ros::ok())
    {
        rate.sleep();
        ros::spinOnce(); 
    }

    serial_port.close();
    receive_thread.join();
    return 0;
}

//串口接收线程
void serial_receive_thread()
{
    odom_t viobot_odom_t;
    int frame_id = 0,last_frame_id = 0;
    while(ros::ok())
    {
        serial_port.read(&receive_buffer[0],1);
        if(receive_buffer[0] == RHEAD_1)
        {
            serial_port.read(receive_buffer + 1,LREV_LEN - 1);
            bool result = unpack_uart(viobot_odom_t,frame_id);
            //拿到了里程计后的操作
            if(frame_id > last_frame_id && result)
            {
                publish_odom(viobot_odom_t);
                last_frame_id = frame_id; //用于判断里程计是否持续更新，可不要  
            }
        }
    }
}

//解析数据包
bool unpack_uart(odom_t& odom,int& frame_id)
{
    if(receive_buffer[0] == RHEAD_1 && receive_buffer[1] == RHEAD_2 && receive_buffer[LREV_LEN - 2] == RTILE_1 && receive_buffer[LREV_LEN - 1] == RTILE_2 )
    {
        unsigned char check=0;
        //61-3
        for(int i = 2;i < LR_CHECK;i++)
        {
            check += receive_buffer[i];
        }

        if(check == receive_buffer[LR_CHECK])
        {
            // std::cout<<"======unpack ok======"<<std::endl;
            // printf("want is %x,but receive:%x\n",check,receive_buffer[LR_CHECK]);

            memcpy(&frame_id,receive_buffer + 2,sizeof(frame_id));
            memcpy(&odom.pose,receive_buffer + 6,sizeof(odom.pose));
            memcpy(&odom.speed,receive_buffer + 6 + sizeof(odom.pose),sizeof(odom.speed));
            // printf("frame_id = %d\n",frame_id);
            //px,py,pz,qx,qy,qz,qw;
            // printf("pose [%f,%f,%f,%f,%f,%f,%f]\n",odom.pose.px,odom.pose.py,odom.pose.pz,odom.pose.qx,odom.pose.qy,odom.pose.qz,odom.pose.qw);
            //lx,ly,lz,ax,ay,az;
            // printf("speed [%f,%f,%f,%f,%f,%f]\n",odom.speed.lx,odom.speed.ly,odom.speed.lz,odom.speed.ax,odom.speed.ay,odom.speed.az);
            return true;
        }
        else
        {
            cout << RED << "Robobaton mini: check err" << TAIL << endl;
            printf("want is %x,but receive:%x\n",check,receive_buffer[LR_CHECK]);
            // printf("frame_id = %d\n",frame_id);
            //px,py,pz,qx,qy,qz,qw;
            //printf("pose [%f,%f,%f,%f,%f,%f,%f]\n",odom.pose.px,odom.pose.py,odom.pose.pz,odom.pose.qx,odom.pose.qy,odom.pose.qz,odom.pose.qw);
            //lx,ly,lz,ax,ay,az;
            // printf("speed [%f,%f,%f,%f,%f,%f]\n",odom.speed.lx,odom.speed.ly,odom.speed.lz,odom.speed.ax,odom.speed.ay,odom.speed.az);
            return false;
        }
    }
    else
    {
        cout << RED << "Robobaton mini: pack format err" << TAIL << endl;
        return false;
    }
}

//发布里程计话题
void publish_odom(const odom_t& odom)
{
    // 坐标转换
    Eigen::Quaterniond q(odom.pose.qw,odom.pose.qx,odom.pose.qy,odom.pose.qz);

    Eigen::Matrix3d RC2C = q.toRotationMatrix();
    Eigen::Matrix3d RI2C(Eigen::AngleAxisd( M_PI * 0.5,Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd( M_PI * 0.5,Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond Q(RC2C * RI2C);
    Q.normalize();
    // Eigen::Vector3d euler_angles = Q.toRotationMatrix().eulerAngles(2,1,0);
    Eigen::Vector3d euler_angles = Q_to_euler(Q);

    // 话题发布
    viobot_odom.header.stamp = ros::Time::now(); 
    viobot_odom.header.frame_id = "viobot";
    viobot_odom.pose.pose.position.x = odom.pose.px;
    viobot_odom.pose.pose.position.y = odom.pose.py;
    viobot_odom.pose.pose.position.z = odom.pose.pz;
    viobot_odom.pose.pose.orientation.x = Q.x();
    viobot_odom.pose.pose.orientation.y = Q.y();   
    viobot_odom.pose.pose.orientation.z = Q.z();
    viobot_odom.pose.pose.orientation.w = Q.w();
    viobot_odom.twist.twist.linear.x = odom.speed.lx;
    viobot_odom.twist.twist.linear.y = odom.speed.ly;
    viobot_odom.twist.twist.linear.z = odom.speed.lz;
    viobot_odom.twist.twist.angular.x = odom.speed.ax;
    viobot_odom.twist.twist.angular.y = odom.speed.ay;
    viobot_odom.twist.twist.angular.z = odom.speed.az;
    pub_viobot_odom.publish(viobot_odom);

    // 打印
    if(ros::Time::now() - time_last > ros::Duration(1))
    {
        cout << GREEN << ">>>>>>>>>>>>>> Vibot mini: " << TAIL << endl;
        //固定的浮点显示
        cout.setf(ios::fixed);
        // setprecision(n) 设显示小数精度为n位
        cout << setprecision(2);
        //左对齐
        cout.setf(ios::left);
        // 强制显示小数点
        cout.setf(ios::showpoint);
        // 强制显示符号
        cout.setf(ios::showpos);
        cout << GREEN << "mini_pos  [X Y Z] : " << odom.pose.px  << " [ m ] " << odom.pose.py  << " [ m ] " << odom.pose.pz  << " [ m ] " << TAIL << endl;
        cout << GREEN << "mini_vel  [X Y Z] : " << odom.speed.lx << " [m/s] " << odom.speed.ly << " [m/s] " << odom.speed.lz << " [m/s] " << TAIL << endl;
        cout << GREEN << "mini_att  [R P Y] : " << euler_angles[0] * 180 / M_PI << " [deg] " << euler_angles[1] * 180 / M_PI << " [deg] " << euler_angles[2] * 180 / M_PI << " [deg] " << TAIL << endl;
        // cout << GREEN << "att_rate  [R P Y] : " << odom.speed.ax * 180 / M_PI   << " [deg/s] " << odom.speed.ay * 180 / M_PI   << " [deg/s] " << odom.speed.az * 180 / M_PI   << " [deg/s] " << TAIL << endl;
        
        time_last = ros::Time::now();
    }
}

// 将四元数转换为旋转矩阵(外旋,x,y,z)
Eigen::Vector3d Q_to_euler(Eigen::Quaterniond Q)
{
    Eigen::Matrix3d R = Q.toRotationMatrix();
    // 提取外旋 X-Y-Z 欧拉角
    double theta = asin(-R(2, 0)); // Pitch
    double psi = atan2(R(1, 0), R(0, 0)); // Yaw
    double phi = atan2(R(2, 1), R(2, 2)); // Roll
    return Eigen::Vector3d(phi, theta, psi);
}


//串口初始化
void serial_init()
{
	try{
        serial_port.setPort(port_name);
        serial_port.setBaudrate(115200);
        serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
        serial_port.setTimeout(timeout);
        serial_port.open();

    }catch(serial::IOException& e){
        std::cerr<<"can't open serial. "<<port_name<<std::endl;
        exit(0);
    }
	if(!serial_port.isOpen()){
        std::cerr<<"serial is note opend"<<std::endl;
		exit(0);
    }
}

void send_command(const algo_state t)
{   
    memset(send_buffer,0,LSEND_LEN);
    send_buffer[0] = 0x67;
    send_buffer[1] = 0x28;
    send_buffer[2] = t;
    char check_sum = 0;
    for(int i = 2;i < LS_CHECK;i++)
    {
        check_sum += send_buffer[i];
    }
    send_buffer[LS_CHECK] = check_sum;
    send_buffer[LSEND_LEN - 2] = 0x09;
    send_buffer[LSEND_LEN - 1] = 0x0d;
    serial_port.write(send_buffer,LSEND_LEN);    //已发送
    // std::cout << "send is" << std::endl;
    // for(int i = 0;i < LSEND_LEN;i++) {
    //     printf("0x%02x ",send_buffer[i]);
    // }
}

void signal_handle(int sig)
{
    ROS_WARN("process disrupt by ctrl c");
    // 关闭算法
    send_command(algo_disable);
    ROS_INFO("Robobaton mini odom disable...");
    ros::shutdown();
}
// int v;
// while(ros::ok()){
//     ROS_INFO("please input command:");
//     std::cin >> v;
//     if(v == 1){
//         ROS_INFO("algo_enable");
//         send_command(algo_enable);
//     }
//     else if(v == 2){
//         ROS_INFO("algo_disable");
//         send_command(algo_disable);
//     }
//     else if(v == 3){
//         ROS_INFO("algo_reset");
//         send_command(algo_restart);
//     }
//     else if(v == 4){
//         ROS_INFO("exit");
//         break;
//     }
//     r.sleep();
//     ros::spinOnce(); 
// }
